/*
 * Copyright (C) 2006 Roman Krashanitsa
 *
 * @author Roman Krashanitsa
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA. 
 *
 */


package pprz.data_components;

import java.awt.geom.Point2D;




public class Aircraft {
	
	public int lastMSG;
	
	public class Position { public float x;  public float y;  public float z;}

	//state indicates part of the structure that is valid, set by message decoder
	public byte state; // set to one or several of the constants
	
	protected String _name;
	protected int _id;
	
	//config files
	protected String _airframe; 
	protected String _radio; 
	protected String _flight_plan;
	protected String _settings;
	protected String _telemetry;
	protected int _color;
	protected float ground_alt;
	
	//protocol settings
	protected Dev2Proto _uplink_dev_prot;

//	protected HashMap<Long,MessageFormat> messageFormat;
	
	
	//current parameters
	
	//from GPS
	protected int gps_mode; //3=3Dfix/byte_mask
	protected float gps_utm_east; //cm
	protected float gps_utm_north; //cm
	protected float gps_course; //decideg
	protected float gps_alt;	// cm
	protected float gps_vel;	// cm/s
	protected float gps_climb;	// cm/s
	protected int gps_itow;	//ms
	protected int gps_utm_zone;
	protected int gps_nb_err;
	
	//from ATTITUDE
	protected float pitch;	// deg
	protected float roll;	// deg
	protected float yaw;	// deg

	//from NAVIGATION
	protected int cur_block;	
	protected int cur_stage;
	protected float desired_course;
	protected float dist2_wp;
	protected float dist2_home;
	
	//from PPRZ_MODE
	protected int ap_mode; //MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE
	protected int ap_gaz; //MANUAL|AUTO_THROTTLE|AUTO_CLIMB|AUTO_ALT
	protected int ap_lateral; //MANUAL|ROLL_RATE|ROLL|COURSE
	protected int ap_horizontal; //WAYPOINT|ROUTE|CIRCLE
	protected int if_calib_mode; //NONE|DOWN|UP
	protected int mcu1_status; //LOST|OK|REALLY_LOST
	protected int lls_calib; //LLS_CALIB_MODE_OFF|LLS_CALIB_MODE_ON
	
	//from BATTERY
	protected float throttle;
	protected float voltage; //1e-1V
	protected float flight_time; //s
	protected int kill_auto_throttle; //bool
	protected float block_time; //s
	protected float stage_time; //s
	protected float energy; //mAh
	
	//from DESIRED
    protected float des_roll;
    protected float des_pitch;
    protected float des_x;
    protected float des_y;
    protected float des_altitude;
    protected float des_climb;

    //from DOWNLINK_STATUS
    protected float rx_bytes_rate;
    protected float rx_msgs_rate;

    
    // ------------GET VARIABLES----------
    public float get_ground_alt(){return ground_alt;}
	//from GPS
	public int get_gps_mode(){return gps_mode;} //3=3Dfix/byte_mask
	public float get_gps_utm_east(){return gps_utm_east;} //cm
	public float get_gps_utm_north(){return gps_utm_north;} //cm
	public float get_gps_course(){return gps_course;} //decideg
	public float get_gps_alt(){return gps_alt;}	// cm
	public float get_gps_vel(){return gps_vel;}	// cm/s
	public float get_gps_climb(){return gps_climb;}	// cm/s
	public int get_gps_itow(){return gps_itow;}	//ms
	public int get_gps_utm_zone(){return gps_utm_zone;}
	public int get_gps_nb_err(){return gps_nb_err;}
	
	//from ATTITUDE
	public float get_pitch(){return pitch;}	// deg
	public float get_roll(){return roll;}	// deg
	public float get_yaw(){return yaw;}	// deg

	//from NAVIGATION
	public int get_cur_block(){return cur_block;}	
	public int get_cur_stage(){return cur_stage;}
	public float get_desired_course(){return desired_course;}
	public float get_dist2_wp(){return dist2_wp;}
	public float get_dist2_home(){return dist2_home;}
	
	//from PPRZ_MODE
	public int get_ap_mode(){return ap_mode;} //MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE
	public int get_ap_gaz(){return ap_gaz;} //MANUAL|AUTO_THROTTLE|AUTO_CLIMB|AUTO_ALT
	public int get_ap_lateral(){return ap_lateral;} //MANUAL|ROLL_RATE|ROLL|COURSE
	public int get_ap_horizontal(){return ap_horizontal;} //WAYPOINT|ROUTE|CIRCLE
	public int get_if_calib_mode(){return if_calib_mode;} //NONE|DOWN|UP
	public int get_mcu1_status(){return mcu1_status;} //LOST|OK|REALLY_LOST
	public int get_lls_calib(){return lls_calib;} //LLS_CALIB_MODE_OFF|LLS_CALIB_MODE_ON
	
	//from BATTERY
	public float get_throttle(){return throttle;}
	public float get_voltage(){return voltage;} //1e-1V
	public float get_flight_time(){return flight_time;} //s
	public int get_kill_auto_throttle(){return kill_auto_throttle;} //bool
	public float get_block_time(){return block_time;} //s
	public float get_stage_time(){return stage_time;} //s
	public float get_energy(){return energy;} //mAh
	
	//from DESIRED
	public float get_des_roll(){return des_roll;}
	public float get_des_pitch(){return des_pitch;}
	public float get_des_x(){return des_x;}
	public float get_des_y(){return des_y;}
	public float get_des_altitude(){return des_altitude;}
	public float get_des_climb(){return des_climb;}

    //from DOWNLINK_STATUS
	public float get_rx_bytes_rate(){return rx_bytes_rate;}
	public float get_rx_msgs_rate(){return rx_msgs_rate;}
	
	
    // ------------SET VARIABLES----------
	//from GPS
	public void set_gps_mode(int gps_mode){this.gps_mode=gps_mode;} //3=3Dfix/byte_mask
	public void set_gps_utm_east(float gps_utm_east){this.gps_utm_east=gps_utm_east;} //cm
	public void set_gps_utm_north(float gps_utm_north){this.gps_utm_north=gps_utm_north;} //cm
	public void set_gps_course(float gps_course){this.gps_course=gps_course;} //decideg
	public void set_gps_alt(float gps_alt){this.gps_alt=gps_alt;}	// cm
	public void set_gps_vel(float gps_vel){this.gps_vel=gps_vel;}	// cm/s
	public void set_gps_climb(float gps_climb){this.gps_climb=gps_climb;}	// cm/s
	public void set_gps_itow(int gps_itow){this.gps_itow=gps_itow;}	//ms
	public void set_gps_utm_zone(int gps_utm_zone){this.gps_utm_zone=gps_utm_zone;}
	public void set_gps_nb_err(int gps_nb_err){this.gps_nb_err=gps_nb_err;}
	
	//from ATTITUDE
	public void set_pitch(float pitch){this.pitch=pitch;}	// deg
	public void set_roll(float roll){this.roll=roll;}	// deg
	public void set_yaw(float yaw){this.yaw=yaw;}	// deg

	//from NAVIGATION
	public void set_cur_block(int cur_block){this.cur_block=cur_block;}	
	public void set_cur_stage(int cur_stage){this.cur_stage=cur_stage;}
	public void set_desired_course(float desired_course){this.desired_course=desired_course;}
	public void set_dist2_wp(float dist2_wp){this.dist2_wp=dist2_wp;}
	public void set_dist2_home(float dist2_home){this.dist2_home=dist2_home;}
	
	//from PPRZ_MODE
	public void set_ap_mode(int ap_mode){this.ap_mode=ap_mode;} //MANUAL|AUTO1|AUTO2|HOME|NOGPS|FAILSAFE
	public void set_ap_gaz(int ap_gaz){this.ap_gaz=ap_gaz;} //MANUAL|AUTO_THROTTLE|AUTO_CLIMB|AUTO_ALT
	public void set_ap_lateral(int ap_lateral){this.ap_lateral=ap_lateral;} //MANUAL|ROLL_RATE|ROLL|COURSE
	public void set_ap_horizontal(int ap_horizontal){this.ap_horizontal=ap_horizontal;} //WAYPOINT|ROUTE|CIRCLE
	public void set_if_calib_mode(int if_calib_mode){this.if_calib_mode=if_calib_mode;} //NONE|DOWN|UP
	public void set_mcu1_status(int mcu1_status){this.mcu1_status=mcu1_status;} //LOST|OK|REALLY_LOST
	public void set_lls_calib(int lls_calib){this.lls_calib=lls_calib;} //LLS_CALIB_MODE_OFF|LLS_CALIB_MODE_ON
	
	//from BATTERY
	public void set_throttle(float throttle){this.throttle=throttle;}
	public void set_voltage(float voltage){this.voltage=voltage;} //1e-1V
	public void set_flight_time(float flight_time){this.flight_time=flight_time;} //s
	public void set_kill_auto_throttle(int kill_auto_throttle){this.kill_auto_throttle=kill_auto_throttle;} //bool
	public void set_block_time(float block_time){this.block_time=block_time;} //s
	public void set_stage_time(float stage_time){this.stage_time=stage_time;} //s
	public void set_energy(float energy){this.energy=energy;} //mAh
	
	//from DESIRED
	public void set_des_roll(float des_roll){this.des_roll=des_roll;}
	public void set_des_pitch(float des_pitch){this.des_pitch=des_pitch;}
	public void set_des_x(float des_x){this.des_x=des_x;}
	public void set_des_y(float des_y){this.des_y=des_y;}
	public void set_des_altitude(float des_altitude){this.des_altitude=des_altitude;}
	public void set_des_climb(float des_climb){this.des_climb=des_climb;}

    //from DOWNLINK_STATUS
	public void set_rx_bytes_rate(float rx_bytes_rate){this.rx_bytes_rate=rx_bytes_rate;}
	public void set_rx_msgs_rate(float rx_msgs_rate){this.rx_msgs_rate=rx_msgs_rate;}

	
	public Position traj[];
	static final public int traj_len=100;
	public int traj_pos;
	public int traj_start;
	public int traj_n;
	
	public Aircraft(int id, int colorId) 
	{
		this._id=id;
		traj_pos=0;
		traj_n=0;
		traj_start=0;
		_color=colorId;
		alloc();
	}
	
	public Aircraft(String name, int id,	String airframe, String radio,
			String flight_plan, String settings, String telemetry, int color)
			{
				_name=name;
				_id=id;
				_airframe=airframe;
				_radio=radio;
				_flight_plan=flight_plan;
				_settings=settings;
				_telemetry=telemetry;
				_color=color;
				traj_pos=0;
				traj_start=0;
				traj_n=0;
				alloc();
			}
	
	protected void alloc()
	{
		traj=new Position[traj_len];
		for (int i=0; i<traj_len; i++) traj[i]=new Position();		
	}

	
	

	
	
	
	//get config file names
	public String get_name(){return _name;}
	public int get_id(){return _id;}
	public String get_airframe(){return _airframe;}
	public String get_radio(){return _radio;}
	public String get_flight_plan(){return _flight_plan;}
	public String get_settings(){return _settings;}
	public String get_telemetry(){return _telemetry;}
	public int get_color(){return _color;}
	
	//get protocols
	public Dev2Proto getUplink(){return this._uplink_dev_prot;}
	public void setUplink(Dev2Proto uplink){this._uplink_dev_prot=uplink;}
	
	
	private Point2D.Double p;
	public void updateTraj()
	{
		p=GTile.UTM2LL(get_gps_utm_north(),get_gps_utm_east());
		traj[traj_pos].x=(float)p.x; 
		traj[traj_pos].y=(float)p.y;
		traj[traj_pos].z=this.gps_alt;
		traj_pos++; if (traj_n<traj_len) traj_n++;
		if (traj_pos==traj_len) traj_pos=0; 
		if (traj_n!=0 && traj_pos==traj_start) traj_start=traj_pos+1;
		if (traj_start==traj_len) traj_start=0;
		
	}
	
//	public void setAttitude(int roll, int heading, int pitch)
//	{this._roll=roll;this._pitch=pitch;this._heading=heading;}
	
//	public void setPos(long lon, long lat, long alt)
//	{
//		this._lat=lat; this._lon=lon; this._alt=alt;
//		if (traj_pos==traj_len) traj_pos=0;
//		else { traj_pos++; traj_n++; }
//		traj[traj_pos].x=lon; 
//		traj[traj_pos].y=lat;
//		traj[traj_pos].z=alt;
//	}
	
	public void setcolor(int color)
	{
		this._color=color;
	}
	
	
	public void sendSetVar(int index, float value) 
	{
		MsgProtocol p=getUplink()._dec;
		Device d=getUplink()._dev;
		if (p instanceof UplinkProtocol && d instanceof UplinkDevice)
			((UplinkProtocol)p).encode_set_var((UplinkDevice)d,this._id, index-1, value);
	}
	public void sendMoveWp() 
	{
		
	}
	public void sendGoToBlock() 
	{
		
	}
	
	
	
	
	
	
	
	
}
